#include <TFile.h>
#include <TRandom.h>
#include <TMath.h>
#include <TGeoManager.h>
#include <TEveGeoNode.h>
#include <TEveManager.h>

#include "CpLog.h"
#include "CpEvent.h"
#include "CpSimulator.h"
#include <TVector3.h>
#include <TGeoMatrix.h>




ClassImp(CpSimulator);

//________________________________________________________________________________________________________________
CpSimulator::CpSimulator(): TObject() ,
    fNumberOfEvents(1),
    fNumberOfTracksInOneEvent(10),
    fCurrentFile(0),
    fOutputFileName("CpSim.root"),
    fGeometryFile("data/geometry/mygeom.root"),
    fTree("cpTree","Coldpig Tree"),
    fEvent(0)
{
//
// Constructor
//

}

//________________________________________________________________________________________________________________
CpSimulator::~CpSimulator()
{
//
// Destructor.
//
}


void CpSimulator::Init()
{

  TGeoManager::Import(fGeometryFile.Data());
  if (!gGeoManager) return;

  fCurrentFile = TFile::Open(fOutputFileName.Data(),"RECREATE");
  fEvent = new CpEvent();
  fTree.Branch("events","CpEvent",&fEvent);
}


void CpSimulator::Finalize()
{
  fCurrentFile->cd();
  fTree.Write();
//   fTree.Print();
  fCurrentFile->Close();
}


void CpSimulator::Simulate()
{
  Init();
  TString vessel("VESSEL");
  Double_t step=1e-2;
//   step=1;
  gGeoManager->SetStep(step);

  TRandom rnd(0);
//   Double_t delta=0.9;
//   Double_t delta2=1;
  const Double_t *xyz;
  TGeoNode*nextNode;
  const Double_t *currentDirection;
//   Double_t *normal;
  Double_t *currentNodeMatrix;
  Double_t divDot;

  TVector3 vCurrentDirection,vCurrentPoint;
  TVector3 vNormal;
  CpTrack *track;
  Int_t num=0;
  for (Long64_t iEvent=0;iEvent<fNumberOfEvents;iEvent++) {
    CpInfo(Form("=== Event %d ===",iEvent));
    for (Long64_t iTrack=0;iTrack<fNumberOfTracksInOneEvent;iTrack++) {
      CpInfo(Form("Track %d",iTrack));
      track = fEvent->AddTrack(iTrack);
//       gGeoManager->InitTrack(rnd.Uniform(0, delta),rnd.Uniform(0, delta),-10,1,1,1);
//       gGeoManager->InitTrack(rnd.Uniform(0, delta),rnd.Uniform(0, delta),-10,rnd.Uniform(0, delta2),rnd.Uniform(0, delta2),1);
      gGeoManager->InitTrack(0,0,-10,1,1,1);
      while (!gGeoManager->IsOutside()) {
        nextNode = gGeoManager->Step(kFALSE);
        if (gGeoManager->IsOutside()) break;
//         CpInfo(Form("%s",nextNode->GetName()));
//         CpInfo(Form("%s",gGeoManager->GetPath()));
        TString nameTmp(nextNode->GetName());

        if (nameTmp.Contains(vessel)) {
          CpInfo(nameTmp.Data());
//           CpInfo(Form("IsEntering with normal"));
          gGeoManager->FindNextBoundary(step);
          CpInfo("1");
          currentDirection = gGeoManager->GetCurrentPoint();
          vCurrentPoint.SetXYZ(currentDirection[0],currentDirection[1],currentDirection[2]);
          vCurrentPoint.Print();
          currentDirection = gGeoManager->GetCurrentDirection();
          vCurrentDirection.SetXYZ(currentDirection[0],currentDirection[1],currentDirection[2]);
          vCurrentDirection.Print();

          currentDirection = gGeoManager->FindNormal();
//           currentDirection = gGeoManager->GetCurrentMatrix()->GetRotationMatrix();
          vNormal.SetXYZ(currentDirection[0],currentDirection[1],currentDirection[2]);
          vNormal.Print();


          TGeoHMatrix *matrix = gGeoManager->GetCurrentMatrix();
          currentNodeMatrix = matrix->GetTranslation();

          const Double_t *glob_pt = gGeoManager->GetCurrentPoint();
          const Double_t *glob_dir = gGeoManager->GetCurrentDirection();
          const Double_t *glob_norm = gGeoManager->FindNormal();
//           const Double_t *glob_norm = gGeoManager->GetCurrentMatrix()->GetRotationMatrix();
          Double_t local_pt[3];
          Double_t local_dir[3];
          Double_t local_norm[3];
          gGeoManager->MasterToLocal(glob_pt,local_pt);
          gGeoManager->MasterToLocalVect(glob_dir,local_dir);
          gGeoManager->MasterToLocalVect(glob_norm,local_norm);
          CpInfo("2");
          vCurrentPoint.SetXYZ(local_pt[0],local_pt[1],local_pt[2]);
          vCurrentPoint.Print();
          vCurrentDirection.SetXYZ(local_dir[0],local_dir[1],local_dir[2]);
          vCurrentDirection.Print();
//           vNormal = vCurrentDirection.Orthogonal();
//           vNormal.SetXYZ(vCurrentDirection.Orthogonal().X(),vCurrentDirection.Orthogonal().X(),vCurrentDirection.Orthogonal().X());
          vNormal.SetXYZ(local_norm[0],local_norm[1],local_norm[2]);
          vNormal.Print();

          divDot = 2*vCurrentDirection.Dot(vNormal)/vNormal.Dot(vNormal);
          CpInfo(Form("IsEntering divDot %f ",divDot));
          if (divDot == 0) {
            vCurrentDirection *=-1;
          } else {
            vCurrentDirection =  vCurrentDirection - vNormal*divDot;
          }
// u=v-2*n*(v,n)/(n,n)
//
          CpInfo(Form("IsEntering angle %f ",vCurrentDirection.Angle(vNormal)*180/TMath::Pi()));
//           vCurrentDirection.Rotate(TMath::Pi(),vNormal);
// vCurrentDirection.Rotate(TMath::Pi(),vNormal);

          local_dir[0] = vCurrentDirection.X();
          local_dir[1] = vCurrentDirection.Y();
          local_dir[2] = vCurrentDirection.Z();

//           local_dir[0] = -vCurrentDirection.Y();
//           local_dir[1] = -vCurrentDirection.Z();
//           local_dir[2] = -vCurrentDirection.X();


          vCurrentDirection.SetXYZ(local_dir[0],local_dir[1],local_dir[2]);
          vCurrentDirection.Print();


//           gGeoManager->FindNextBoundary(step);
// //           normal = gGeoManager->FindNormal();
//           vNormal.SetXYZ(normal[0],normal[1],normal[2]);
//           vNormal.Print();

//           matrix->ReflectX(kTRUE);
//           matrix->ReflectY(kTRUE);
//           matrix->ReflectZ(kTRUE);
//           matrix-> ReflectZ(kFALSE);
//           CpInfo(Form("IsEntering with currentNodeMatrix %f %f %f %d",currentNodeMatrix[0],currentNodeMatrix[1],currentNodeMatrix[2],matrix->IsReflection()));
//           gGeoManager->SetCurrentDirection(currentNodeMatrix[0],currentNodeMatrix[1],currentNodeMatrix[2]);
          Double_t glob_pt_new[3];
          Double_t glob_dir_new[3];
          gGeoManager->LocalToMaster(local_pt,glob_pt_new);
          gGeoManager->LocalToMasterVect(local_dir,glob_dir_new);
          CpInfo("3");

          vCurrentPoint.SetXYZ(glob_pt_new[0],glob_pt_new[1],glob_pt_new[2]);
          vCurrentPoint.Print();
          //           currentDirection = gGeoManager->GetCurrentDirection();
          vCurrentDirection.SetXYZ(glob_dir_new[0],glob_dir_new[1],glob_dir_new[2]);
          vCurrentDirection.Print();

          gGeoManager->SetCurrentDirection(glob_dir_new);

          /*
          //           gGeoManager->MasterToLocal(gGeoManager->GetCurrentPoint(),)
                    gGeoManager->MasterToLocalVect(gGeoManager->GetCurrentDirection(),currentNodeMatrix);
                    currentDirection = gGeoManager->GetCurrentDirection();
                    vCurrentDirection.SetXYZ(currentDirection[0],currentDirection[1],currentDirection[2]);
                    vCurrentDirection.Print();
                    vNormal.Print();
                    divDot = 2*vNormal.Dot(vCurrentDirection)/vNormal.Dot(vNormal);
                    CpInfo(Form("IsEntering divDot %f ",divDot));
                    vCurrentDirection = vCurrentDirection - vNormal*divDot;
                    vCurrentDirection.Print();
          //             vCurrentDirection.Rotate(TMath::Pi(),vNormal);
          //           vCurrentDirection *=-1;
          //           vCurrentDirection.RotateX(-1);
          //           vCurrentDirection.RotateY(vNormal.Y());
          //           vCurrentDirection.RotateZ(vNormal.Z());


          //           vCurrentDirection.Rotate(0,-vNormal);
          // vCurrentDirection.Rotate(vCurrentDirection.Angle(vNormal)+TMath::Pi(),vNormal);
                    CpInfo(Form("IsEntering angle %f ",vCurrentDirection.Angle(vNormal)*180/TMath::Pi()));
          //           vCurrentDirection.Rotate(3*TMath::Pi()/2,vNormal);
                    CpInfo(Form("IsEntering with currentDirection after %f %f %f",vCurrentDirection.X(),vCurrentDirection.Y(),vCurrentDirection.Z()));
          //           gGeoManager->SetCurrentDirection(-vCurrentDirection.X()*TMath::Cos(vNormal.X()),-vCurrentDirection.Y()*TMath::Cos(vNormal.Y()),vCurrentDirection.Z()*TMath::Cos(vNormal.Z()));

          //           vCurrentDirection.Dot(vNormal);
          //           gGeoManager->SetCurrentDirection(-vCurrentDirection.X(),-vCurrentDirection.Y(),vCurrentDirection.Z());
                    gGeoManager->SetCurrentDirection(vCurrentDirection.X(),vCurrentDirection.Y(),vCurrentDirection.Z());
          //           nextNode = gGeoManager->Step(kFALSE);
          //           gGeoManager->SetCurrentDirection(normal);
          //           break;*/

          num++;
          if (num>10) break;

        }

        xyz = gGeoManager->GetCurrentPoint();
//         CpInfo(Form("%f %f %f",xyz[0],xyz[1],xyz[2]));
        track->AddPoint(xyz[0],xyz[1],xyz[2],0.0);
      }

//       track->AddPoint(iTrack*0.1,iTrack*0.1,iTrack*1.0,0.0);
//       track->AddPoint(iTrack*0.1,iTrack*0.2,iTrack*2.0,1.0);
//       track->AddPoint(iTrack*0.1,iTrack*0.3,iTrack*3.0,2.0);
    }
    fTree.Fill();
    fEvent->Clear();
  }
  Finalize();
}


void CpSimulator::ShowEvent(const Long64_t& eventNum)
{

  TGeoManager::Import("macros/mygeom.root");
  if (!gGeoManager) return;


  TEveManager::Create();

  TGeoNode* node = gGeoManager->GetTopNode();
  TEveGeoTopNode* en = new TEveGeoTopNode(gGeoManager, node);
  en->SetVisLevel(4);
  en->GetNode()->GetVolume()->SetVisibility(kFALSE);

  gEve->AddGlobalElement(en);

  gEve->Redraw3D(kTRUE);


  TFile *f = TFile::Open(fOutputFileName.Data(),"READ");
  TTree *tree = (TTree*) f->Get("cpTree");
  CpEvent *ev = 0;
//   tree->Print();
  tree->SetBranchAddress("events",&ev);
  tree->GetEntry(eventNum);
  ev->Print();





//   for(Long64_t iEvent=0;iEvent<fNumberOfEvents;iEvent++) {
//     tree->GetEntry(i);
//     ev->Print();
//   }

}

